#include "IzdvajanjeStatike.h"

IzdvajanjeStatike::IzdvajanjeStatike(CvCapture *gornjaKamera, CvCapture *bocnaKamera)
{
	_gornjaKamera = gornjaKamera;
	_bocnaKamera = bocnaKamera;

	_brojMirnihFrameovaGornja = DEFAULT_BROJ_MIRNIH_FRAMEOVA;
	_brojMirnihFrameovaBocna = DEFAULT_BROJ_MIRNIH_FRAMEOVA;

	_pragStatikeGornja = DEFAULT_PRAG_STATIKE;
	_pragDinamikeGornja = DEFAULT_PRAG_DINAMIKE;

	_pragStatikeBocna = DEFAULT_PRAG_STATIKE;
	_pragDinamikeBocna = DEFAULT_PRAG_DINAMIKE;

	_koristiROIgornja = false;
	_koristiROIbocna = false;

	_koristiOtklanjanjeSuma = false;

	_prikaziBocnuDinamiku = false;
	_prikaziGornjuDinamiku = false;
	_prikaziBocnuStatiku = false;
	_prikaziGornjuStatiku = false;

	_stop = false;
	_stopped = false;

	_gornjaMiruje = false;
	_bocnaMiruje = false;

	_koristiFiksniFPS= false;
	_fiksniFPS = DEFAULT_FPS;
	
	_handleGornjaStatika = NULL;
	_handleBocnaStatika = NULL;
	_handleObjeStatike = NULL;
}

IzdvajanjeStatike::~IzdvajanjeStatike()
{

}

void IzdvajanjeStatike::PostaviBrojMirnihFrameova(int gornja, int bocna)
{
	_brojMirnihFrameovaGornja = gornja;
	_brojMirnihFrameovaBocna = bocna;
}

void IzdvajanjeStatike::PostaviGornjePragove(double pragStatike, double pragDinamike)
{
	/*  Histereza */
	_pragStatikeGornja = pragStatike;
	_pragDinamikeGornja = pragDinamike;
}

void IzdvajanjeStatike::PostaviBocnePragove(double pragStatike, double pragDinamike)
{
	/*  Histereza */
	_pragStatikeBocna = pragStatike;
	_pragDinamikeBocna = pragDinamike;
}

void IzdvajanjeStatike::PostaviFiksniFPS(int fps)
{
	_fiksniFPS = fps;
}

void IzdvajanjeStatike::KoristiFiksniFPS(void)
{
	_koristiFiksniFPS = true;
}

void IzdvajanjeStatike::PostaviROIGornja(int x, int y, int width, int height)
{
	_gornjaROI = cvRect(x, y, width, height);
	_koristiROIgornja = true;
}

void IzdvajanjeStatike::PostaviROIBocna(int x, int y, int width, int height)
{
	_bocnaROI = cvRect(x, y, width, height);
	_koristiROIbocna = true;
}

void IzdvajanjeStatike::KoristiOtklanjanjeSuma(void)
{
	_koristiOtklanjanjeSuma = true;
}

void IzdvajanjeStatike::Stop(void)
{
	_stop = true;

	/* Cekaj da uredno ubije thread */
	do
	{

#ifdef WINDOWS_OS
		Sleep(5);
#else
		usleep(5000);
#endif

	}
	while(_stopped == false);
}

bool IzdvajanjeStatike::IsStopped(void)
{
	return _stopped;
}


void IzdvajanjeStatike::PostaviOpcijePrikaza(bool prikaziBocnuDinamiku, bool prikaziGornjuDinamiku, bool prikaziBocnuStatiku, bool prikaziGornjuStatiku)
{
	_prikaziBocnuDinamiku = prikaziBocnuDinamiku;
	_prikaziBocnuStatiku = prikaziBocnuStatiku;
	_prikaziGornjuDinamiku = prikaziGornjuDinamiku;
	_prikaziGornjuStatiku = prikaziGornjuStatiku;

	if(_prikaziBocnuDinamiku)
	{
		cvNamedWindow(IME_BOCNA_DINAMIKA, CV_WINDOW_AUTOSIZE); 
		cvMoveWindow(IME_BOCNA_DINAMIKA, PROZOR_POZICIJA_1, PROZOR_POZICIJA_1);
	}

	if(_prikaziGornjuDinamiku)
	{
		cvNamedWindow(IME_GORNJA_DINAMIKA, CV_WINDOW_AUTOSIZE); 
		cvMoveWindow(IME_GORNJA_DINAMIKA, PROZOR_POZICIJA_2, PROZOR_POZICIJA_1);
	}

	if(_prikaziBocnuStatiku)
	{
		cvNamedWindow(IME_BOCNA_STATIKA, CV_WINDOW_AUTOSIZE); 
		cvMoveWindow(IME_BOCNA_STATIKA, PROZOR_POZICIJA_1, PROZOR_POZICIJA_2);
	}

	if(_prikaziGornjuStatiku)
	{
		cvNamedWindow(IME_GORNJA_STATIKA, CV_WINDOW_AUTOSIZE); 
		cvMoveWindow(IME_GORNJA_STATIKA, PROZOR_POZICIJA_2, PROZOR_POZICIJA_2);
	}
}


void IzdvajanjeStatike::Pokreni()
{

#ifdef WINDOWS_OS

	LPDWORD dwThreadID = 0;
	CreateThread(NULL,0,(LPTHREAD_START_ROUTINE)DoWork, (void *)this ,0,dwThreadID);

#else

	 pthread_t thread;
	 pthread_create(&thread, NULL, DoWork, (void *)this);

#endif

}




/* Glavna dretva */

#ifdef WINDOWS_OS
void IzdvajanjeStatike::DoWork(void *p)
#else
void* IzdvajanjeStatike::DoWork(void *p)
#endif
{
	IzdvajanjeStatike *klasa = (IzdvajanjeStatike*)p;

	/* Tekuca slika */
	IplImage* imgGornja = 0; 
	IplImage* imgBocna = 0; 

	/* Tekuca slika, ali obradjena da ne bude suma */
	IplImage* imgGornjaSmooth = 0; 
	IplImage* imgBocnaSmooth = 0; 

	/* Slika sa pocetka perioda mirovanja - sve unutar (potencijalnog) perioda mirovanja se usporedjuju s njom */
	IplImage* imgGornjaInicijalna = 0; 
	IplImage* imgBocnaInicijalna = 0; 

	/* Tekuca razlika tekuce slike i inijcalne slike potencijalnog perioda mirovanja */
	IplImage* imgRazlikaGornja = 0;
	IplImage* imgRazlikaBocna= 0;

	/* Izlazna slika koja se isporucuje sustavu */
	IplImage* imgGornjaIzlazna = 0; 
	IplImage* imgBocnaIzlazna = 0; 

	/* Privremena izlazna slika koja se isporucuje sustavu - uzima se slika iz sredine perioda mirovanja */
	IplImage* imgGornjaIzlaznaTemp = 0; 
	IplImage* imgBocnaIzlaznaTemp = 0; 

	/* Skalarni zapis razlike */
	CvScalar razlikaG;
	CvScalar razlikaB;

	/* RGB u jedan double */
	double razlikaG_total;
	double razlikaB_total;

	int fpsGornja;
	int fpsBocna;


	if(klasa->_koristiFiksniFPS == true)
	{
		fpsGornja = klasa->_fiksniFPS;
		fpsBocna = klasa->_fiksniFPS;
	}
	else
	{
		fpsGornja = (int)cvGetCaptureProperty(klasa->_gornjaKamera, CV_CAP_PROP_FPS);
		fpsBocna = (int)cvGetCaptureProperty(klasa->_bocnaKamera, CV_CAP_PROP_FPS);
	}




	/* Worker */
	while(klasa->_stop == false)
	{
 
		if(!cvGrabFrame(klasa->_bocnaKamera))
		{             
			klasa->_stop = true;
			break;
		}

		if(!cvGrabFrame(klasa->_gornjaKamera))
		{             
			klasa->_stop = true;
			break;
		}

		imgBocna=cvRetrieveFrame(klasa->_bocnaKamera);
		imgGornja=cvRetrieveFrame(klasa->_gornjaKamera);

		
		if(klasa->_prikaziBocnuDinamiku)
			cvShowImage(IME_BOCNA_DINAMIKA, imgBocna);

		if(klasa->_prikaziGornjuDinamiku)
			cvShowImage(IME_GORNJA_DINAMIKA, imgGornja);


		/* INIT */
		if(imgBocnaInicijalna == 0)
		{
			klasa->_trenutnoStatickihBocnih = 0;

			imgBocnaInicijalna= cvCreateImage(cvSize(imgBocna->width, imgBocna->height),
											imgBocna->depth,
											imgBocna->nChannels);

			// Smooth operation (remove camera noise)
			if(klasa->_koristiOtklanjanjeSuma)
				cvSmooth(imgBocna, imgBocnaInicijalna, CV_BLUR, 5, 5); 
			else
				cvCopy(imgBocna, imgBocnaInicijalna, NULL);

			imgRazlikaBocna = cvCreateImage(cvSize(imgBocnaInicijalna->width, imgBocnaInicijalna->height),
											imgBocnaInicijalna->depth,
											imgBocnaInicijalna->nChannels);

			if(klasa->_koristiROIbocna)
					cvSetImageROI(imgRazlikaBocna, klasa->_bocnaROI);

			imgBocnaIzlazna = cvCreateImage(cvSize(imgBocnaInicijalna->width, imgBocnaInicijalna->height),
											imgBocnaInicijalna->depth,
											imgBocnaInicijalna->nChannels);

			imgBocnaIzlaznaTemp = cvCreateImage(cvSize(imgBocnaInicijalna->width, imgBocnaInicijalna->height),
											imgBocnaInicijalna->depth,
											imgBocnaInicijalna->nChannels);


			imgBocnaSmooth = cvCreateImage(cvSize(imgBocnaInicijalna->width, imgBocnaInicijalna->height),
											imgBocnaInicijalna->depth,
											imgBocnaInicijalna->nChannels);
		}

		/* INIT */
		if(imgGornjaInicijalna == 0)
		{
			klasa->_trenutnoStatickihGornjih = 0;

			imgGornjaInicijalna= cvCreateImage(cvSize(imgGornja->width, imgGornja->height),
											imgGornja->depth,
											imgGornja->nChannels);


			// Smooth operation (remove camera noise)
			if(klasa->_koristiOtklanjanjeSuma)
				cvSmooth(imgGornja, imgGornjaInicijalna, CV_BLUR, 3, 3); 
			else
				cvCopy(imgGornja, imgGornjaInicijalna, NULL);

			imgRazlikaGornja = cvCreateImage(cvSize(imgGornjaInicijalna->width, imgGornjaInicijalna->height),
											imgGornjaInicijalna->depth,
											imgGornjaInicijalna->nChannels);

			if(klasa->_koristiROIgornja)
					cvSetImageROI(imgRazlikaGornja, klasa->_gornjaROI);


			imgGornjaIzlazna = cvCreateImage(cvSize(imgGornjaInicijalna->width, imgGornjaInicijalna->height),
											imgGornjaInicijalna->depth,
											imgGornjaInicijalna->nChannels);

			imgGornjaIzlaznaTemp = cvCreateImage(cvSize(imgGornjaInicijalna->width, imgGornjaInicijalna->height),
											imgGornjaInicijalna->depth,
											imgGornjaInicijalna->nChannels);

			imgGornjaSmooth = cvCreateImage(cvSize(imgGornjaInicijalna->width, imgGornjaInicijalna->height),
											imgGornjaInicijalna->depth,
											imgGornjaInicijalna->nChannels);

	
		}



		/* Razlika */
		//cvSub(imgGornja, imgGornjaInicijalna, imgRazlikaGornja, NULL);
		//cvSub(imgBocna, imgBocnaInicijalna, imgRazlikaBocna, NULL);

		if(klasa->_koristiOtklanjanjeSuma)
		{
			cvSmooth(imgBocna, imgBocnaSmooth, CV_BLUR, 5, 5); 
			cvSmooth(imgGornja, imgGornjaSmooth, CV_BLUR, 5, 5); 

			if(klasa->_koristiROIbocna)
						cvSetImageROI(imgBocnaSmooth, klasa->_bocnaROI);
			if(klasa->_koristiROIgornja)
						cvSetImageROI(imgGornjaSmooth, klasa->_gornjaROI);
		}
		else
		{
			if(klasa->_koristiROIbocna)
						cvSetImageROI(imgBocna, klasa->_bocnaROI);
			if(klasa->_koristiROIgornja)
						cvSetImageROI(imgGornja, klasa->_gornjaROI);
		}

		if(klasa->_koristiROIbocna)
					cvSetImageROI(imgBocnaInicijalna, klasa->_bocnaROI);
		if(klasa->_koristiROIgornja)
					cvSetImageROI(imgGornjaInicijalna, klasa->_gornjaROI);


		if(klasa->_koristiOtklanjanjeSuma)
		{
			cvAbsDiff(imgGornjaSmooth, imgGornjaInicijalna, imgRazlikaGornja);
			cvAbsDiff(imgBocnaSmooth, imgBocnaInicijalna, imgRazlikaBocna);

			cvResetImageROI(imgGornjaSmooth);
			cvResetImageROI(imgBocnaSmooth);
		}
		else
		{
			cvAbsDiff(imgGornja, imgGornjaInicijalna, imgRazlikaGornja);
			cvAbsDiff(imgBocna, imgBocnaInicijalna, imgRazlikaBocna);

			cvResetImageROI(imgGornja);
			cvResetImageROI(imgBocna);
		}


		cvResetImageROI(imgGornjaInicijalna);
		cvResetImageROI(imgBocnaInicijalna);

		/* Probaj izgladit sum kamere i na razlici */
		if(klasa->_koristiOtklanjanjeSuma)
		{
			cvSmooth(imgRazlikaGornja, imgRazlikaGornja, CV_BLUR, 5, 5); 
			cvSmooth(imgRazlikaBocna, imgRazlikaBocna, CV_BLUR, 5, 5); 
		}

		razlikaG = cvSum(imgRazlikaGornja);
		razlikaB = cvSum(imgRazlikaBocna);

		razlikaG_total = razlikaG.val[0] + razlikaG.val[1] + razlikaG.val[2];
		razlikaB_total = razlikaB.val[0] + razlikaB.val[1] + razlikaB.val[2];


		/* Evaluacija mirovanja gornje */
		/* Mini-histereza je eksperimentalna */
		if(razlikaG_total > klasa->_pragDinamikeGornja)
		{

			// Smooth operation (remove camera noise)
			if(klasa->_koristiOtklanjanjeSuma)
				cvSmooth(imgGornja, imgGornjaInicijalna, CV_BLUR, 3, 3); 
			else
				cvCopy(imgGornja, imgGornjaInicijalna, NULL);
			
			klasa->_trenutnoStatickihGornjih = 0;
			klasa->_gornjaMiruje = false;
		}
		else if(razlikaG_total < klasa->_pragStatikeGornja)
		{
			if(klasa->_gornjaMiruje == false)
			{
				(klasa->_trenutnoStatickihGornjih)++;

				// Treba uzeti srednji uzorak iz perioda mirovanja (za "optimalni" rezultat)
				if(klasa->_trenutnoStatickihGornjih == (klasa->_brojMirnihFrameovaGornja / 2))
					cvCopy(imgGornja, imgGornjaIzlaznaTemp, NULL);
			}

			if((klasa->_gornjaMiruje == false) && 
				(klasa->_trenutnoStatickihGornjih) >= (klasa->_brojMirnihFrameovaGornja))
			{
				klasa->_gornjaMiruje = true;

				cvCopy(imgGornjaIzlaznaTemp, imgGornjaIzlazna, NULL);


				//if(klasa->_koristiROIgornja)
				//	cvSetImageROI(imgGornjaIzlazna, klasa->_gornjaROI);

				
				if(klasa->_prikaziGornjuStatiku)
					cvShowImage(IME_GORNJA_STATIKA, imgGornjaIzlazna);

				
				if(klasa->_handleGornjaStatika != NULL)
						klasa->_handleGornjaStatika(imgGornjaIzlazna);

				if(klasa->_bocnaMiruje == true)
					if(klasa->_handleObjeStatike != NULL)
						klasa->_handleObjeStatike(imgGornjaIzlazna, imgBocnaIzlazna);

			}
		}

	


		/* Evaluacija mirovanja bocne */
		/* Mini-histereza je eksperimentalna */
		if(razlikaB_total > klasa->_pragDinamikeBocna)
		{

			// Smooth operation (remove camera noise)
			if(klasa->_koristiOtklanjanjeSuma)
				cvSmooth(imgBocna, imgBocnaInicijalna, CV_BLUR, 5, 5);
			else
				cvCopy(imgBocna, imgBocnaInicijalna, NULL);
			
			klasa->_trenutnoStatickihBocnih = 0;
			klasa->_bocnaMiruje = false;
		}
		else if(razlikaB_total < klasa->_pragStatikeBocna)
		{
			if(klasa->_bocnaMiruje == false)
			{
				(klasa->_trenutnoStatickihBocnih)++;

				// Treba uzeti srednji uzorak iz perioda mirovanja (za "optimalni" rezultat)
				if(klasa->_trenutnoStatickihBocnih == (klasa->_brojMirnihFrameovaBocna / 2))
					cvCopy(imgBocna, imgBocnaIzlaznaTemp, NULL);

			}

			if((klasa->_bocnaMiruje == false) && 
				(klasa->_trenutnoStatickihBocnih) >= (klasa->_brojMirnihFrameovaBocna))
			{
				klasa->_bocnaMiruje = true;

				cvCopy(imgBocnaIzlaznaTemp, imgBocnaIzlazna, NULL);

				//if(klasa->_koristiROIbocna)
				//		cvSetImageROI(imgBocnaIzlazna, klasa->_bocnaROI);
				

				if(klasa->_prikaziBocnuStatiku)
					cvShowImage(IME_BOCNA_STATIKA, imgBocnaIzlazna);

				
				if(klasa->_handleBocnaStatika != NULL)
						klasa->_handleBocnaStatika(imgBocnaIzlazna);


				if(klasa->_gornjaMiruje == true)
					if(klasa->_handleObjeStatike != NULL)
						klasa->_handleObjeStatike(imgGornjaIzlazna, imgBocnaIzlazna);
					

			}
		}


#ifdef STATIKA_DEBUG
		printf("\rGornja razlika: %f\t\tBocna razlika: %f",razlikaG_total, razlikaB_total);
#endif
		

		/* Delay */
#ifdef WINDOWS_OS

		if(fpsGornja > fpsBocna)
		{
			Sleep(1000/fpsBocna);
		}
		else
		{
			Sleep(1000/fpsGornja);
		}
#else
		if(fpsGornja > fpsBocna)
		{
			usleep((unsigned long)(1000000.0/fpsBocna));
		}
		else
		{
			usleep((unsigned long)(1000000.0/fpsGornja));
		}
#endif


	}

	klasa->_stopped = true;
}


void IzdvajanjeStatike::HandleBocnuStatiku(dogadjajBocnaStatika handle)
{
	_handleBocnaStatika = handle;
}

void IzdvajanjeStatike::HandleGornjuStatiku(dogadjajGornjaStatika handle)
{
	_handleGornjaStatika = handle;
}

void IzdvajanjeStatike::HandleObjeStatike(dogadjajObjeStatike handle)
{
	_handleObjeStatike = handle;
}

